//
// Created by lustre on 2023/1/15.
//

#ifndef QUADRUPED_QUADRUPED_H
#define QUADRUPED_QUADRUPED_H
// 舵机的转动范围,对于PCA9685输入的PWM值范围,SG90 MG90S符合此, 更换舵机请修改

#ifndef QUADRUPED_SERVO_SETTINGS
#define BASE_POS 100  // 基线
#define END_POS 500   // 上限

// pwm频率
#define PWM_FREQUENCY 50
// 单步延时，同一个舵机两次转动之间的延时。当上一次动作结束后，定时器才能唤醒执行下一步动作，故需要设置此延时
#define step_delay  150
#endif

// 将指定的x 在 0 ~ 100的数字映射到 舵机转动范围 base ~ end
// y = k * x + a
// a = base 舵机起点
// k = range * x / 100
// range = end - base
// y = (end -base) * x / 100 + base
#define mappingToPwmValue(x) ((END_POS - BASE_POS) * x / 100 + BASE_POS)

#include "Adafruit_PWMServoDriver.h"


enum KneeConvergence {
    LEFT_CONVERGENCE = 0,      // 左值收敛
    MIDPOINT_CONVERGENCE = 1  // 中位线收敛

};

struct Direction {
    uint8_t flags;
    bool operator[](int index) const {
        if (index < 0 || index > 7) {
            return false;
        }
        return (flags >> (7 - index)) & 1;
    }
};

/**
 * 所有单位均为百分比
 * */
struct QuadrupedVariantConf {
    int leftLimit;
    int rightLimit;
    int hipResetPos;

    int upperLimit;
    int lowerLimit;
    int kneeResetPos;

    KneeConvergence kneeConvergence;

    int stepHigh;
    int stepLength;
    Direction direction;

};


class Joint {
public:
    // 舵机驱动板的pin 0~15
    int pin;
    // 以下上限不必大于下限，只是在前进的方向上 ，往腿前迈的极限为上限 腿往上提的极限为上限
    // 机械结构转动上限   0 ~ 100
    int upperLimit;
    // 机械结构转动下限   0 ~ 100
    int lowerLimit;
    /**
     * 复位点
     * */
    int resetPos;

    virtual void turnStart() = 0;
    virtual void turnEnd() = 0;

    Joint(int pin, int upperLimit, int lowerLimit, int resetPos, bool direction);

};

/**
 * 髋关节
 * */
class HipJoint : public Joint {

public:

    // 走路时摆动的范围
    int leftWalkingLimit{};

    int rightWalkingLimit{};

    int realRightLimit{};

    int realLeftLimit{};

    void left();

    void right();

    void turnStart() override {
        left();
    }

    void turnEnd() override {
        right();
    }

    HipJoint(int pin, const QuadrupedVariantConf &quadrupedVariantConf, int flagIndex);

    void setStepLength(uint8_t rate);
};

/**
 * 膝关节 与髋关节数据字段并无太大区别 ，但使用不同名称 命名属性和函数便于区分具体操作行为和具体对象
 * */
class KneeJoint : public Joint {

public:

    int realUpperLimit{};

    int realLowerLimit{};

    int nowUpper{};

    int nowLower{};

    /**
     * 提腿
     * */
    void up();

    /**
     * 落脚
     * */
    void down();

    void turnStart() override {
        down();
    }

    void turnEnd() override {
        up();
    }

    /**
     * 设置提腿高度比例
     * @param rate 1~ 100
     * */
    void setStepHigh(uint8_t rate, KneeConvergence kneeConvergence);

    KneeJoint(int pin, const QuadrupedVariantConf &quadrupedVariantConf, int flagIndex);

};

class Quadruped {
public:
    explicit Quadruped(const QuadrupedVariantConf &variantConf);


    void init(int sda, int scl);

    /**
     * 当前函数是同步的。需要当前线程异步
     * */
    void doAsyncAction(int action);

    /**
    * @brief 控制转向
    *
    * @param target 目标舵机
    * @param pos pwm值 范围 BASE_POS ~ END_POS
    *
    */
    void turn(uint8_t target, uint16_t pos);
    /**
    * @brief 控制转向
    *
    * @param target 目标舵机
    * @param pos pwm值 范围 BASE_POS ~ END_POS
    *
    */
    void turnLimit(uint8_t target, bool start);

    void stop();

    void reset();

    volatile int getQuadrupedAction() const;

    void setQuadrupedAction(volatile int action);

    void (Quadruped::*allPinActionTab[3])() = {&Quadruped::stop, &Quadruped::reset,
                                               &Quadruped::sitDown
    };

    /**
     * 云台上下转动
     * @param pos 0 ~ 100
     * */
    void setUpDownGimbalPos(uint8_t pos);

    /**
     * 云台左右转动，可能硬件不支持
     * @param pos 0 ~ 100
     * */
    void setLeftRightGimbalPos(uint8_t pos);

    /**
     * 1 ~ 50 相当于提腿的高度，1为10%  5为50% 则齐腰
     * */
    void setStepHigh(uint8_t rate);

    /**
      * 1 ~ 100 步子大小，为限位转动范围的比例
      * */
    void setStepLength(uint8_t rate);


    /*
     * 偏航角 左右偏向比例 -100 ~ 100 默认 0  ;
     * <0 降低左脚的比例 > 0 降低右脚比例
     * 不影响步长值，被控制脚的摆动范围 会在步长值基础上 进一步收缩
     * */
    void setYawRate(uint8_t rate);


    void (Quadruped::*robotSettings[6])(uint8_t) = {&Quadruped::setUpDownGimbalPos, &Quadruped::setLeftRightGimbalPos,
                                                    &Quadruped::setStepHigh, &Quadruped::setStepLength,
                                                    &Quadruped::setYawRate
    };

    /**
    * 设置当前动作的提腿高度但不保存，不影响下一个动作
    */
    void setStepHighOnce(uint8_t rate);

    /**
      *设置当前动作的跨步步长比，不影响下一个动作
      * */
    void setStepLengthOnce(uint8_t rate);

private:
    //左前腿
    HipJoint frontLeftLeg;
    //左前膝盖
    KneeJoint frontLeftKnee;
    //右前腿
    HipJoint rightFrontLeg;
    //右前膝盖
    KneeJoint rightFrontKnee;
    //右后腿
    HipJoint rightRearLeg;
    //右后膝盖
    KneeJoint rightRearKnee;
    //左后腿
    HipJoint rearLeftLeg;
    //左后膝盖
    KneeJoint rearLeftKnee;

    KneeConvergence kneeConvergence;

    Joint *joints[8] = {&frontLeftLeg, &frontLeftKnee,
                        &rightFrontLeg, &rightFrontKnee,
                        &rightRearLeg, &rightRearKnee,
                        &rearLeftLeg, &rearLeftKnee};


    int volatile quadrupedAction = -1;

    /**
     *步长，用于切换动作时，复位前进后退的重心补偿对其他动作的影响
     * */

    uint8_t stepLength = 80;

    uint8_t stepHigh = 30;


    /*
     * 左右偏向比例 -100 ~ 100 默认 0
     * */
    int yawRate = 0;


    void forward();

    void backward();

    void turnRight();

    void turnLeft();

    void fastForward();

    /**
     * 坐下
     * */
    void sitDown();


/**
 * 动作表，这里由Ticker调用
 * 通过 {@link Quadruped#setQuadrupedAction()}
 * 设置具体动作，Ticker在下一次唤醒时调用
 * */
    void (Quadruped::*actionTab[5])() = {
            &Quadruped::forward, &Quadruped::backward,
            &Quadruped::turnRight, &Quadruped::turnLeft,
            &Quadruped::fastForward
    };

};


#endif //QUADRUPED_QUADRUPED_H
